#include "cartographer/mapping/imu_tracker.h"

#include <cmath>
#include <limits>

#include "cartographer/common/math.h"
#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"

namespace cartographer
{
    namespace mapping
    {
        /*
        静止时加速计输出向量向上，读数为9.8;无外力加速时，加速计能准确输出ROLL和PITCH两轴姿态角度且不会有累积误差,在更长时间尺度
        内都是准确的;但加速计测角度的缺点是其用MEMS技术检测惯性力造成的微小形变，而惯性力与重力本质是一样的,所以加速计就不会区分
        重力加速度与外力加速度，当系统在三维空间做变速运动时，其输出就不正确了。

        陀螺仪的角速度是瞬时量，在姿态平衡时不能直接使用；需要通过角速度与时间积分计算角度变化量，将角度变化量加上初始角度得到目
        标角度；积分时间越小输出角度越精确，但陀螺仪原理决定了它的测量基准是自身，并没有系统外的绝对参照物，且积分时间不可能无限
        小，故积分的误差会随着时间累积迅速增加，所以陀螺仪只能工作在相对较短的时间尺度内。

        在无其它参照物时，要得到较真实姿态角，要利用加权算法（一阶低通滤波）来扬长避短,在短时间内增加陀螺仪的权值，在长时间内增加
        加速度权值，这样系统输出角度就接近真实值了。
        */

        ImuTracker::ImuTracker(const double imu_gravity_time_constant, const common::Time time)
            : imu_gravity_time_constant_(imu_gravity_time_constant)
            , time_(time)
            , last_acc_time_(common::Time::min())
            , orientation_(Eigen::Quaterniond::Identity())
            , gravity_vector_(Eigen::Vector3d::UnitZ())
            , imu_angular_velocity_(Eigen::Vector3d::Zero())
        {
        }

        // rotation.conjugate()就是就是逆向旋转，即姿态的逆;
        void ImuTracker::Advance(const common::Time time)
        {
            CHECK_LE(time_, time);
            const double delta_t = common::ToSeconds(time - time_);
            const Eigen::Quaterniond rotation =
                transform::AngleAxisVectorToRotationQuaternion(Eigen::Vector3d(imu_angular_velocity_ * delta_t));
            orientation_ = (orientation_ * rotation).normalized();
            // 重力方向与姿态角同步更新,只不过重力方向是反向旋转;gravity_vector_并不是真正的重力向量，而是表示初始时运动平面的垂直方向，其存在的意义是为了矫正
            // 根据陀螺仪角速度积分得到的姿态角，所以这里重力向量和姿态角同步更新
            gravity_vector_ = rotation.conjugate() * gravity_vector_;
            time_ = time;
        }

        // https://blog.csdn.net/m0_67976789/article/details/130159078
        // 使用滤波后的加速方向矫正姿态角
        void ImuTracker::AddImuLinearAccelerationObservation(const Eigen::Vector3d &imu_linear_acceleration)
        {
            double deltaTime = common::ToSeconds(time_ - last_acc_time_);
            const double delta_t = last_acc_time_ > common::Time::min() ? deltaTime : std::numeric_limits<double>::infinity();
            last_acc_time_ = time_;
            // 因陀螺仪的角速在短期内准，长期内累积误差大，而加速度测的姿态角在长期内是准的；所以delta_t越大，陀螺仪输出姿态角误差
            // 越大，此时当前加速测量值的权重越大，即越相信当前的重力方向，即尽量用当前的重力方向来纠正姿态角
            const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);
            // 一阶低通滤波法采用本次采样值与上次滤波输出值进行加权，得到有效滤波值，使得输出对输入有反馈作用；防止加速剧烈变化
            gravity_vector_ = (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;
            const Eigen::Quaterniond rotation = FromTwoVectors(gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());
            orientation_ = (orientation_ * rotation).normalized();
            CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
            CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);
        }

        void ImuTracker::AddImuAngularVelocityObservation(const Eigen::Vector3d &imu_angular_velocity)
        {
            imu_angular_velocity_ = imu_angular_velocity;
        }

    } // namespace mapping
} // namespace cartographer
